#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "move_group_interface_tutorial");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // BEGIN_TUTORIAL 入门教程
  //
  // Setup 步骤
  // ^^^^^
  //
  // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
  // the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
  // are used interchangably.
  //moveit关节操作组被称作“计划组（planning groups）”，并将他们并将它们存储在一个名为“JointModelGroup”的对象中。 
  //在整个 MoveIt 中，术语“计划组”和“联合模型组”可互换使用。
  static const std::string PLANNING_GROUP = "gluon";

  // The :planning_interface:`MoveGroupInterface` class can be easily
  // setup using just the name of the planning group you would like to control and plan for.
  //:planning_interface:`MoveGroupInterface` 类可以使用您想要控制和计划的计划组的名称轻松设置。
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

  // We will use the :planning_interface:`PlanningSceneInterface`
  // class to add and remove collision objects in our "virtual world" scene
  //我们将使用类（planning_interface:`PlanningSceneInterface）在我们的“虚拟世界”场景中添加和删除碰撞对象

  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // Raw pointers are frequently used to refer to the planning group for improved performance.
  //原始指针经常用于引用计划组以提高性能。
  const moveit::core::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // Visualization 可视化
  // ^^^^^^^^^^^^^
  //
  // The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
  //MoveItVisualTools 包提供了许多功能，用于在 RViz 中可视化对象,机器人和轨迹以及调试工具,例如脚本的逐步内省
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("dummy");
  visual_tools.deleteAllMarkers();

  // Remote control is an introspection tool that allows users to step through a high level script
  // via buttons and keyboard shortcuts in RViz
  //远程控制是一种内省工具，允许用户通过 RViz 中的按钮和键盘快捷键逐步执行高级脚本
  visual_tools.loadRemoteControl();

  // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
  //RViz 提供了多种类型的标记，在这个演示中我们将使用文本、圆柱体和球体
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
  //批量发布用于减少发送到 RViz 进行大型可视化的消息数量
  visual_tools.trigger();

  // Getting Basic Information 获取基本信息
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // We can print the name of the reference frame for this robot.
  //我们可以打印这个机器人的参考系名称。
  ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  //我们还可以打印该组的末端执行器链接的名称。

  ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // We can get a list of all the groups in the robot:我们也可以得到机器人的所有的组列表
  ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
  std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
            std::ostream_iterator<std::string>(std::cout, ", "));

while(ros::ok()) {

  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to go to home");
  std::vector<double> joint_home_positions(6, 0.0);
  move_group.setJointValueTarget(joint_home_positions);
  ROS_INFO("Go to home");
  move_group.move();

  // Start the demo 开始demo
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

  // Planning to a Pose goal 规划到目标位置
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // We can plan a motion for this group to a desired pose for the
  // end-effector.我们可以将该组的运动规划为末端执行器所需的姿势。
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.18;
  target_pose1.position.y = -0.12;
  target_pose1.position.z = 0.25;
  move_group.setPoseTarget(target_pose1);

  // Now, we call the planner to compute the plan and visualize it.
  // Note that we are just planning, not asking move_group
  // to actually move the robot.
  //调用planner来计算计划并将其可视化（只是规划，还没有具体控制运动）
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  bool success = false;
#if 0
  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

  if (success)
    move_group.execute(my_plan.trajectory_);
  // Visualizing plans  可视化规划
  // ^^^^^^^^^^^^^^^^^
  // We can also visualize the plan as a line with markers in RViz.
  //在RVIZ中我们也能够用线可视化轨迹
  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
  visual_tools.publishAxisLabeled(target_pose1, "pose1");
  visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
#endif

  // Moving to a pose goal 移动到目标姿态
  // ^^^^^^^^^^^^^^^^^^^^^
  //
  // Moving to a pose goal is similar to the step above 
  // except we now use the move() function. Note that
  // the pose goal we had set earlier is still active
  // and so the robot will try to move to that goal. We will
  // not use that function in this tutorial since it is
  // a blocking function and requires a controller to be active
  // and report success on execution of a trajectory.
  //移动到姿势目标与上面的步骤类似，除了我们现在使用 move() 函数。
  // 请注意，我们之前设置的姿势目标仍然处于活动状态，因此机器人将尝试移动到该目标。 
  //我们不会在本教程中使用该函数，
  //因为它是一个阻塞函数，需要一个控制器处于活动状态并报告轨迹执行成功。

  /* Uncomment below line when working with a real robot */
  /* move_group.move(); */

#if 0
  // Planning to a joint-space goal 关节空间目标规划
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // Let's set a joint space goal and move towards it.  This will replace the
  // pose target we set above.
  //
  //让我们设置一个关节空间的目标，移动他。这将取代我们上面设置的目标姿态。
  // To start, we'll create an pointer that references the current robot's state.
  //开始，我们创建一个参考当前的机器人状态的点
  // RobotState is the object that contains all the current position/velocity/acceleration data.
  //机器人状态（robotstate）是一个对象包含当前的位置，速度，加速度数据
  moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
  //
  // Next get the current set of joint values for the group.
  //接下来得到当前的对于当前组的关节集合的数值
  std::vector<double> joint_group_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
  for(int i=0; i<joint_group_positions.size(); i++)
    ROS_INFO("%.4f", joint_group_positions[i]);

  // Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
  //现在，让我们更改一个关节，规划一个新的关节空间目标同时可视化规划。
  joint_group_positions[0] = -1.0;  // radians
  move_group.setJointValueTarget(joint_group_positions);

  // We lower the allowed maximum velocity and acceleration to 5% of their maximum.
  //我们降低允许的最大速度和加速度为最大值的5%
  // The default values are 10% (0.1).默认值是10%
  // Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config
  //设置你更好的默认值在你的机器人配置joint_limits.yaml文件
  // or set explicit factors in your code if you need your robot to move faster.
  //如果你需要让你的机器人移动的更快，在你的代码中设置明确的参数。
  move_group.setMaxVelocityScalingFactor(0.05);
  move_group.setMaxAccelerationScalingFactor(0.05);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");

  if (success)
    move_group.execute(my_plan.trajectory_);
  // Visualize the plan in RViz
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");
#endif

#if 0
  // Planning with Path Constraints                约束路径的规划
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // Path constraints can easily be specified for a link on the robot.
  //路径约束能够容易的指定在机器人的连杆
  // Let's specify a path constraint and a pose goal for our group.
  //让我们指定路径约束和对于我们组的目标姿态。
  // First define the path constraint. 首先定义路径约束
  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = "6_Link";
  ocm.header.frame_id = "base_link";
  ocm.orientation.w = 1.0;
  ocm.absolute_x_axis_tolerance = 0.1;
  ocm.absolute_y_axis_tolerance = 0.1;
  ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 1.0;

  // Now, set it as the path constraint for the group.
  //现在，设置它作为组的路径约束。
  moveit_msgs::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(test_constraints);

  // We will reuse the old goal that we had and plan to it.
  //我们将重用我们已有的旧目标并计划实现它。
  // Note that this will only work if the current state already
  // satisfies the path constraints. So we need to set the start
  // state to a new pose.
  //请注意，这仅在当前状态已经满足路径约束时才有效。
  // 所以我们需要将开始状态设置为一个新的姿势。
  moveit::core::RobotState start_state(*move_group.getCurrentState());
#endif

  geometry_msgs::Pose start_pose2;
  start_pose2.orientation.y = 0.70711;
  start_pose2.orientation.w = 0.70711;
  start_pose2.position.x = 0.30;
  start_pose2.position.y = 0.0;
  start_pose2.position.z = 0.10;

#if 0
  start_state.setFromIK(joint_model_group, start_pose2);
  move_group.setStartState(start_state);

  // Now we will plan to the earlier pose target from the new
  // start state that we have just created.
  //现在我们将更容易从我们已经创建的开始状态规划目标位姿
  move_group.setPoseTarget(target_pose1);

  // Planning with constraints can be slow because every sample must call an inverse kinematics solver.
  //使用约束进行规划可能会很慢，因为每个样本都必须调用逆运动学求解器。
  // Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
  //让我们将计划时间从默认的 5 秒增加，以确保计划者有足够的时间成功。
  move_group.setPlanningTime(100.0);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

  if (success)
    move_group.execute(my_plan.trajectory_);
  // Visualize the plan in RViz
  visual_tools.deleteAllMarkers();
  visual_tools.publishAxisLabeled(start_pose2, "start");
  visual_tools.publishAxisLabeled(target_pose1, "goal");
  visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");

  // When done with the path constraint be sure to clear it.
  //完成路径约束后，请务必清除它。
  move_group.clearPathConstraints();
#endif

  // Cartesian Paths  笛卡尔坐标系路径
  // ^^^^^^^^^^^^^^^
  // You can plan a Cartesian path directly by specifying a list of waypoints
  // for the end-effector to go through. Note that we are starting
  // from the new start state above.  The initial pose (start state) does not
  // need to be added to the waypoint list but adding it can help with visualizations
  //您可以通过指定末端执行器要经过的路标列表来直接规划笛卡尔路径。 
  //请注意，我们是从上面的新开始状态开始的。 
  //初始姿势（开始状态）不需要添加到航点列表，但添加它可以帮助可视化
  std::vector<geometry_msgs::Pose> waypoints;
  waypoints.push_back(start_pose2);

  geometry_msgs::Pose target_pose3 = start_pose2;

  move_group.setPoseTarget(start_pose2);
  ROS_INFO("Move to start_pose2");
  move_group.move();

  //target_pose3.position.z -= 0.12;
  target_pose3.position.x += 0.10;
  waypoints.push_back(target_pose3);  // down (push_back)在vector类中作用为在vector尾部加入一个数据。

  target_pose3.position.y += 0.15;
  waypoints.push_back(target_pose3);  // right

  //target_pose3.position.z += 0.05;
  target_pose3.position.y -= 0.10;
  //target_pose3.position.x += 0.05;
  waypoints.push_back(target_pose3);  // up and left

  // We want the Cartesian path to be interpolated at a resolution of 1 cm
  // which is why we will specify 0.01 as the max step in Cartesian
  // translation.  We will specify the jump threshold as 0.0, effectively disabling it.
  // Warning - disabling the jump threshold while operating real hardware can cause
  // large unpredictable motions of redundant joints and could be a safety issue
  //我们希望笛卡尔路径以 1 厘米的分辨率进行插值，这就是为什么我们将指定 0.01 作为笛卡尔平移的最大步长。
   //我们将跳跃阈值指定为 0.0，从而有效地禁用它。
   //警告 - 在操作真实硬件时禁用跳跃阈值会导致冗余关节的大量不可预测的运动，并且可能是一个安全问题
  moveit_msgs::RobotTrajectory trajectory;
  const double jump_threshold = 0.0;
  const double eef_step = 0.01;
  double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

  // Visualize the plan in RViz             在rviz中可视化规划
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Cartesian Path", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
  for (std::size_t i = 0; i < waypoints.size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // Cartesian motions should often be slow, e.g. when approaching objects. The speed of cartesian
  // plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time
  // the trajectory manually, as described [here](https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4).
  // Pull requests are welcome.
  //笛卡尔运动通常应该很慢，例如 接近物体时。
  // 笛卡尔计划的速度目前无法通过 maxVelocityScalingFactor 设置，但需要您手动为轨迹计时，
  //如 [此处(https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4) 所述。 欢迎拉取请求。

  // You can execute a trajectory like this.      你能够像这样执行一个轨迹。
  int nrOfPoints = trajectory.joint_trajectory.points.size();
  ROS_INFO("nrOfPoints %d", nrOfPoints);
  for (int i = 0; i < nrOfPoints; i++) {
    //ActuatorController::UnifiedID actuator = uIDArray.at(i);
    int nrOfJoints = trajectory.joint_trajectory.points[i].positions.size();
    for (int j = 0; j < nrOfJoints; j++) {
      ROS_INFO("point[%d]: %s, %f", i, trajectory.joint_trajectory.joint_names[j].c_str(), 
          trajectory.joint_trajectory.points[i].positions[j]);
    }
    ROS_INFO("-------------------------------------------");
  }
  move_group.execute(trajectory);
  ros::Duration(2).sleep();

#if 0
  // Adding/Removing Objects and Attaching/Detaching Objects
  //增加、移除物体和吸附分离物体
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // Define a collision object ROS message. 定义一个碰撞物体ros消息
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group.getPlanningFrame();

  // The id of the object is used to identify it. 盒子的id被用来定于它
  collision_object.id = "box1";

  // Define a box to add to the world.      定义盒子在世界坐标系下增加。
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 0.4;
  primitive.dimensions[1] = 0.1;
  primitive.dimensions[2] = 0.4;

  // Define a pose for the box (specified relative to frame_id)
  //定义一个盒子的位姿（指定参考的坐标系）
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = 0.4;
  box_pose.position.y = -0.2;
  box_pose.position.z = 1.0;

  collision_object.primitives.push_back(primitive);
  collision_object.primitive_poses.push_back(box_pose);
  collision_object.operation = collision_object.ADD;

  std::vector<moveit_msgs::CollisionObject> collision_objects;
  collision_objects.push_back(collision_object);

  // Now, let's add the collision object into the world     现在，让我们将碰撞物体增加到世界坐标系下。
  ROS_INFO_NAMED("tutorial", "Add an object into the world");
  planning_scene_interface.addCollisionObjects(collision_objects);

  // Show text in RViz of status    展示text在rviz状态
  visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  // Wait for MoveGroup to receive and process the collision object message
  //等待 MoveGroup 接收并处理碰撞对象消息 
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

  // Now when we plan a trajectory it will avoid the obstacle       现在当我们规划一个轨迹的时候就会避免碰撞它
  move_group.setStartState(*move_group.getCurrentState());
  geometry_msgs::Pose another_pose;
  another_pose.orientation.w = 1.0;
  another_pose.position.x = 0.4;
  another_pose.position.y = -0.4;
  another_pose.position.z = 0.9;
  move_group.setPoseTarget(another_pose);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");

  // Visualize the plan in RViz 
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");

  // Now, let's attach the collision object to the robot.       现在，让我们黏贴上一个碰撞物体到机器人上
  ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
  move_group.attachObject(collision_object.id);

  // Show text in RViz of status          展示text在rviz状态中
  visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for MoveGroup to receive and process the attached collision object message */
  //等到Movegroup组收到和处理黏贴物体的消息
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the "
                      "robot");

  // Now, let's detach the collision object from the robot.         现在让我们从机器人分离碰撞物体
  ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
  move_group.detachObject(collision_object.id);

  // Show text in RViz of status
  visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for MoveGroup to receive and process the attached collision object message */
   //等到Movegroup组收到和处理黏贴物体的消息
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object detaches to the "
                      "robot");

  // Now, let's remove the collision object from the world.        现在，让我们从世界坐标系中移除碰撞物体
  ROS_INFO_NAMED("tutorial", "Remove the object from the world");
  std::vector<std::string> object_ids;
  object_ids.push_back(collision_object.id);
  planning_scene_interface.removeCollisionObjects(object_ids);

  // Show text in RViz of status
  visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();
  /* Wait for MoveGroup to receive and process the attached collision object message */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
#endif

  // END_TUTORIAL

#if 0
  // Start the circle demo              开始画圆演示
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the circle demo");

    std::string eef_link = move_group.getEndEffector();
    std::string reference_frame = "base_link";
    move_group.setPoseReferenceFrame(reference_frame);

    move_group.allowReplanning(true);
    move_group.setPlanningTime(100.0);

    move_group.setGoalPositionTolerance(0.001);
    move_group.setGoalOrientationTolerance(0.01);
    move_group.setMaxAccelerationScalingFactor(0.8);
    move_group.setMaxVelocityScalingFactor(0.8);

    // go to home
    move_group.setNamedTarget("home_pose");
    move_group.move();
    ros::Duration(1).sleep();

    geometry_msgs::Pose target_pose;
    target_pose.orientation.x = 0;
    target_pose.orientation.y = 0.70711;
    target_pose.orientation.z = 0;
    target_pose.orientation.w = 0.70711;

    target_pose.position.x = 0.10;
    target_pose.position.y = 0.20;
    target_pose.position.z = 0.30;

    move_group.setPoseTarget(target_pose);
    move_group.move();

    std::vector<geometry_msgs::Pose> waypoints_1;
    waypoints_1.push_back(target_pose);

    // in xy place create a circle
    double centerA = target_pose.position.y;
    double centerB = target_pose.position.z;
    double radius = 0.10;

    for(double theta = 0.0; theta < M_PI*2; theta += 0.01)
    {
        target_pose.position.y = centerA + radius * cos(theta);
        target_pose.position.z = centerB + radius * sin(theta);
        waypoints_1.push_back(target_pose);
    }

    // Cartesian path
    //moveit_msgs::RobotTrajectory trajectory;
    fraction = 0.0;
    int maxtries = 500;   // max retry count  最大尝试次数
    int attempts = 0;     // already done retry count 已经允许的尝试次数

    while(fraction < 1.0 && attempts < maxtries)
    {
        fraction = move_group.computeCartesianPath(waypoints_1, eef_step, jump_threshold, trajectory);
        attempts++;

        if(attempts % 10 == 0)
            ROS_INFO("Still trying after %d attempts...", attempts);
    }

    if(fraction == 1)
    {
        ROS_INFO("Path computed successfully. Moving the arm.");

        moveit::planning_interface::MoveGroupInterface::Plan plan;
        plan.trajectory_ = trajectory;

        move_group.execute(plan);
        ros::Duration(1).sleep();
    }
    else
    {
        ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);
    }
#endif

  ros::Duration(10).sleep();
}
  ros::shutdown();
  return 0;
}
